#ifndef VELPOSE_SIM_CORE_H
#define VELPOSE_SIM_CORE_H

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_eigen/tf2_eigen.h>
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "tf/tf.h"
#include "nav_msgs/Odometry.h"

#define __APP_NAME__ "velpose_sim"

class VelPoseSim{
    public:
        VelPoseSim();
        ~VelPoseSim();
        void run();

    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
    
    private:
        double init_x, init_y, init_yaw;
        std::string input_ctrl_cmd_topic_;
    
    private:
        ros::Subscriber ctrl_cmd_sub_;
        /* 订阅initial_pose*/
        ros::Subscriber initial_pose_sub;
    
    private:
        std::shared_ptr<geometry_msgs::TwistStamped> ctrl_cmd_;

    private:
        tf2_ros::TransformBroadcaster transformBroadcaster;
        tf2_ros::Buffer buffer;
        tf2_ros::TransformListener listener;

    
    private:
        void ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
        void process(const ros::TimerEvent& e);
};

#endif